#ifndef __GOTO_H__
#define __GOTO_H__

#include <atomic>
#include <chrono>
#include <iostream>
#include <typeinfo>
#include <vector>

#include "plate_solver/plate_solver.h"
#include "motor_controller.h"
#include "ws_constant.h"
#include "rgb_power.h"

struct MotorParameter {
    double angle;
    double speed;
    bool direction;
    int resolution_level;
    int speed_ramping;

    MotorParameter() : angle(0.0), speed(0.0), direction(0), resolution_level(0), speed_ramping(0) {}

    MotorParameter(double p_angle, double p_speed, bool p_direction, int p_resolution_level, int p_speed_ramping) : 
    angle(p_angle), speed(p_speed), direction(p_direction), resolution_level(p_resolution_level), speed_ramping(p_speed_ramping) {}
};

struct MotorPath
{
    MotorParameter motor_yaw_para;
    MotorParameter motor_pitch_para;

    MotorPath(const MotorParameter& p_motor_yaw_para, const MotorParameter& p_motor_pitch_para) : 
    motor_yaw_para(p_motor_yaw_para), motor_pitch_para(p_motor_pitch_para) {}
};

class AstroTarget {
protected:
    HourAngleCoordinates zenith_coord_ = {0.0, 90.0};// 默认天顶为北天极
    std::string target_name_;

public:
    AstroTarget() {}
    AstroTarget(const HourAngleCoordinates& zenith_coord, const std::string& target_name) : zenith_coord_(zenith_coord), target_name_(target_name) {}
    virtual void myCoordAtTime(const std::chrono::system_clock::time_point& now, HorizontalCoordinates& myCoord) = 0;
    virtual const HourAngleCoordinates& getZenithCoord() const { return zenith_coord_; }
    virtual void getTargetName(std::string& target_name) const { target_name = target_name_; }
    virtual void getTargetCoordJ2000(EquatorialCoordinates& taget_coord_j2000) const { 
        taget_coord_j2000.ra = 0.0;
        taget_coord_j2000.dec = 0.0;
    };
};

class SolarGalaxyObject : public AstroTarget {
private:
    SolarSystemObjectIndex target_Index_ = EarthsMoon;

public:
    SolarGalaxyObject() {}
    SolarGalaxyObject(const SolarSystemObjectIndex& target_Index, HourAngleCoordinates& zenith_coord, const std::string& target_name)
    : target_Index_(target_Index), AstroTarget(zenith_coord, target_name) {}

    void myCoordAtTime(const std::chrono::system_clock::time_point& now, HorizontalCoordinates& myCoord);
};

class DeepSkyObject : public AstroTarget {
private:
    EquatorialCoordinates target_eq_coord_J2000_ = {0.0, 0.0};

public:
    DeepSkyObject() {}
    DeepSkyObject(const EquatorialCoordinates& target_eq_coord_J2000, const HourAngleCoordinates& zenith_coord, const std::string& target_name)
    : target_eq_coord_J2000_(target_eq_coord_J2000), AstroTarget(zenith_coord, target_name) {}

    void getTargetCoordJ2000(EquatorialCoordinates& taget_coord_j2000) const { taget_coord_j2000 = target_eq_coord_J2000_; }

    void myCoordAtTime(const std::chrono::system_clock::time_point& now, HorizontalCoordinates& myCoord);
};

class ITips;

class Goto {
private:
    static AstroTarget* target_;
    static DeepSkyObject dso_;
    static SolarGalaxyObject sgo_;

    RgbPower rgb_power_;

    HorizontalCoordinates target_my_coord_ = {0.0, 0.0};
    
    HorizontalCoordinates last_mechanical_coord_ = {0.0, 0.0};
    HorizontalCoordinates last_my_coord_ = {0.0, 0.0};
    HorizontalCoordinates now_mechanical_coord_ = {0.0, 0.0};
    HorizontalCoordinates now_my_coord_ = {0.0, 0.0};

    StepMotorUser& motor_yaw_;
    StepMotorUser& motor_pitch_;

    const double motor_high_speed_ = 32;
    const double motor_low_speed_ = 1;
    const int motor_high_speed_resolution_level_ = STEP_MOTOR_MINISTEP_8;
    const int motor_low_speed_resolution_level_ = STEP_MOTOR_MINISTEP_32;
    const int motor_track_resolution_level_ = STEP_MOTOR_MINISTEP_256;
    const int motor_speed_ramping_ = STEP_MOTOR_RAMP_PULSE_0;// 去掉加减速，加减速时间不好确定

    const double motor_pitch_up_reset_offset_angle = 26 + 7.2;  // 向上光电限位距离90度向下的位置的角度差
    const double motor_pitch_down_reset_offset_angle = 7.2;  // 向下光电限位距离90度向下的位置的角度差
    bool b_reverse_motor_pitch_direction_ = false;

    const double exposure_ = 1.0;
    const int gain_ = 80;
    const int ir_state_ = 1;// IR PASS

    AstrometryNet asNetPlateSolver_;
    PlateSolver plateSolver_;

    CamTele cam_tele_;

    std::atomic<bool> b_goto = false;
    std::atomic<bool> b_tracking = false;

    static std::atomic<StateNotify> gotoer_state_;
    static std::atomic<StateNotify> tracker_state_;

    bool b_reverse_motor_yaw_mechanical_direction_;
    bool b_reverse_motor_pitch_mechanical_direction_;

    ITips* itips_observer_;

private:
    int updateNowCoord();
    int gotoTargetConsiderBacklash();
    int gotoTargetConsiderMotorRunTime(const HorizontalCoordinates& now_my_coord, MotorPath& path);
    int pathPlanning(const HorizontalCoordinates& now_my_coord, MotorPath& path);
    int trackTarget();
    int trackSunMoon();
    int sunMoonHoughCircles(const cv::Mat& img, std::vector<cv::Vec3f>& circles);
    int getSunMoonCenterPos(cv::Vec3f& circle);
    int teleViewPixelCoordToMechanicalCoord(const cv::Point& pixel_coord, HorizontalCoordinates& mechanical_coord, const HorizontalCoordinates& center_mechanical_coord);

public:
    Goto() : plateSolver_(&asNetPlateSolver_), motor_yaw_(MotorController::motorYaw()), motor_pitch_(MotorController::motorPitch()) {}
    inline void setGpsPosition(const GeocentricCoordinates& gps_position) { 
        Ephemeris::setLocationOnEarth(gps_position.lat, gps_position.lon); 
        Ephemeris::flipLongitude(false);//东正西负
    }

    inline void setTargetToNull() { target_ = nullptr; }
    inline void setDeepSkyObject(const DeepSkyObject& dso) { 
        dso_ = dso; 
        target_ = &dso_;
    }
    inline void setSolarGalaxyObject(const SolarGalaxyObject& sgo) {
        sgo_ = sgo;
        target_ = &sgo_;
    }
    inline void setLastMechanicalCoord(const HorizontalCoordinates& last_mechanical_coord) { last_mechanical_coord_ = last_mechanical_coord; }
    inline void setLastMyCoord(const HorizontalCoordinates& last_my_coord) { last_my_coord_ = last_my_coord; }

    inline void getTargetName(std::string& target_name) const { 
        if (target_ != nullptr)
            target_->getTargetName(target_name); 
    }
    inline void getTargetCoordJ2000(EquatorialCoordinates& taget_coord_j2000) const { 
        if (target_ != nullptr)
            target_->getTargetCoordJ2000(taget_coord_j2000); 
    }
    inline bool isSolarGalaxyObject() const {
        if (target_ == nullptr) {
            return false;
        } else {
            if (typeid(*target_) == typeid(SolarGalaxyObject))
                return true;
            else
                return false;
        }
    }
    inline bool isGotoObject() const {
        if (target_ == nullptr) 
            return false;
        else
            return true;
    }
    inline StateNotify getGotoerState() const { return gotoer_state_; }
    inline StateNotify getTrackerState() const { return tracker_state_; }

    void setAndNotifyGotoerState(const StateNotify& state);
    void notifyGotoerState();
    void setAndNotifyTrackerState(const StateNotify& state);
    void notifyTrackerState();
    void setAndNotifySpecialTrackerState(const StateNotify& state);
    void notifySpecialTrackerState();

    int gotoAndTarckTarget();
    int trackTargetConsiderBacklash();
    int trackSpecialTarget();
    int stopGotoAndTracking();
    int stopGoto();
    int stopTrackingSpecial();

    void setReverseMotorYawMechanicalDirection(bool b_reverse_motor_yaw_mechanical_direction) { b_reverse_motor_yaw_mechanical_direction_ = b_reverse_motor_yaw_mechanical_direction; }
    void setReverseMotorPitchMechanicalDirection(bool b_reverse_motor_pitch_mechanical_direction) { b_reverse_motor_pitch_mechanical_direction_ = b_reverse_motor_pitch_mechanical_direction; }

    inline void addITipsObserver(ITips* itips_observer) { itips_observer_ = itips_observer; }
};



#endif